/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;


/**
 *
 * @author Mark
 */
public class Lifter {

    private static Kicker instance = null;
    private Joystick lifterJoy;
    private int state;
    private boolean doHookCylinder;     // Extend (Up) Hook Cylinder
    private Solenoid sol01;             // Slot 8 Output 1
    private double winchMotorSpeed;
    private Victor winchMotor;

    long startTime;                     // Memorized System Milliseconds Elapsed
    long curTime;                       // Current System Milliseconds Elapsed
    long deltaTime;                     // Difference in Milliseconds between Start and Current

    private boolean manualMode;

    public Lifter (Joystick shooter)
    {
        lifterJoy = shooter;
        manualMode = false;
        state = 0;

        sol01 = new Solenoid(1);
        winchMotor = new Victor(4,9);

    }


    void Run()
    {

        // ----------------------------------------
        // Control the Hook Cylinder

        if( lifterJoy.getRawButton(6) )
            doHookCylinder = true;
        if( lifterJoy.getRawButton(7))
            doHookCylinder = false;



        // ----------------------------------------
        // Control the Winch Motor


        double tempSpeed = lifterJoy.getY();
        double absTempSpeed = Math.abs(tempSpeed);


        if( absTempSpeed < 0.25 )     // dead band
            winchMotorSpeed = 0.0;
        else
        {
            if( tempSpeed < 0.0 )
                winchMotorSpeed = ( absTempSpeed - 0.25 ) / 0.75;
            else
                winchMotorSpeed = -( absTempSpeed - 0.25 ) / 0.75;
        }
            

        if(lifterJoy.getRawButton(8))
            System.out.println("lifterJoy.getY = " + tempSpeed);
            //System.out.println("winchMotorSpeed = " + winchMotorSpeed);
        



        // ----------------------------------------
        // Update the Physical Outputs

        // Must be pressing the unlock buttons on the kicker's joystick. See
        // Utils for the exact buttons.
        if(isLockOff())
        {
            sol01.set(doHookCylinder);
            winchMotor.set(winchMotorSpeed);
        }
        else
        {
            sol01.set(false);
            winchMotor.set(0);    
        }


    }

    /**
     * Check to see if the kicker's joystick is pressing the appropriate unlock
     * buttons to prevent something bad from happening.
     * @return True if the buttons are pressed, false otherwise
     */
    private boolean isLockOff()
    {
        if(lifterJoy.getRawButton(Utils.LIFTER_LOCK_BUTTON_NUM_1) &&
           lifterJoy.getRawButton(Utils.LIFTER_LOCK_BUTTON_NUM_2))
        {
            return true;
        }
        else
        {
            return false;
        }
    }
}
